#include "main.h"
#include "mpu6050.h"
#include "pid.h"
#include "sys.h"

extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;
extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim4;
extern char RxBuffer;
float Pitch,Roll,Pitch_dot,Roll_dot;		//角度变量
short accx,accy,accz,gyrox,gyroy,gyroz;			//陀螺仪原始数据

int ctr_x1=134,ctr_y1=256,ctr_x2=123,ctr_y2=125,
		ctr_angle_x,ctr_angle_y,ctr_skewing,ctr_skewing1,ctr_skewing2,ctr_accelerator,  //遥控处理参数
		exp_angle_x,exp_angle_y,exp_skewing;  //期望参数

//懂得都懂
void run()
{
	printf("开机成功！\r\n");
	Fly_Mode = Manual;
	HAL_Delay(2000);
    MPU_Init();


	HAL_UART_Receive_IT(&huart1, &RxBuffer, 1);
	while(1)  //开机保护
	{
		if(ctr_y1<1010)
		{
			break;
		}
	}
	printf("保护关闭！\r\n");

	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,1000);
//	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,1000);
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,1000);
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_4,1000);
	__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,1000);
	HAL_Delay(500);
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
//	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
	HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
	HAL_TIM_Base_Start_IT(&htim4);
	HAL_TIM_Base_Start_IT(&htim2);
	while(1)
	{
	}
}


static PID angle_skewing_pid;
static PID angle_y_pid;

HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  UNUSED(htim);

  if(htim == &htim2)
  {
	//	Get_KalmanAngle(&Pitch,&Pitch_dot,'P');		//获得Pitch角度与角速度
		Get_KalmanAngle(&Roll,&Roll_dot,'R');			//获得Roll角度与角速度
	//	printf("\r\n%0.1f %0.1f",Pitch,Roll);
  }

  angle_skewing_pid.Kp = 6;
  angle_skewing_pid.Kd = 0.1;

  angle_y_pid.Kp = 5;
  angle_y_pid.Kd = 1;


  if(htim == &htim4)
  {
	  static int time=0;

	  if(time++ == 4)
	  {
		  time = 0;
		  ctr_accelerator = (ctr_y1-14)*5 + 1000;  //油门
		  ctr_angle_x = (ctr_x2-123)*5;   //偏航
		  exp_angle_y = (ctr_y2-123)*5;   //俯仰
		  exp_skewing = (ctr_x1-134);   //横滚


		  //判断是否自稳定模式
		  if(Fly_Mode==0)
		  {
			  ctr_skewing = exp_skewing;
		  }
		  else if(Fly_Mode==1)
		  {
			  angle_skewing_pid.setpoint = exp_skewing;
			  PIDPosition(&angle_skewing_pid, Roll*3);
			  ctr_skewing = angle_skewing_pid.result;
		  }
		  //pid自稳定

		  //翘头pid
//		  angle_y_pid.setpoint = exp_angle_y;
//		  PIDPosition(&angle_y_pid, -Pitch);
//		  ctr_angle_y = angle_y_pid.result;

		  ctr_skewing1 = ctr_skewing-exp_angle_y;  //控制俯仰
		  ctr_skewing2 = ctr_skewing+exp_angle_y;


		  //限幅
		  if(ctr_accelerator>2000)ctr_accelerator=2000;
		  if(ctr_accelerator<1000)ctr_accelerator=1000;
		  if(ctr_angle_y<-800)ctr_angle_y=-800;
		  if(ctr_angle_y>800)ctr_angle_y=800;
		  if(ctr_angle_x<-800)ctr_angle_x=-800;
		  if(ctr_angle_x>800)ctr_angle_x=800;
		  if(ctr_angle_x<-800)ctr_angle_x=-800;
		  if(ctr_angle_x>800)ctr_angle_x=800;
		  if(ctr_skewing<-800)ctr_skewing=-800;
		  if(ctr_skewing>800)ctr_skewing=800;



		  //输出pwm
		 __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,1000-ctr_angle_x);
//		 __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,1000-ctr_angle_x);
		 __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,1000-ctr_skewing1);
		 __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_4,1000-ctr_skewing2);

		 __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,ctr_accelerator);
	  }



  }
}



